#include "functions.h"

//distanceCount and sensorCount are essentially lookup tables generated
//by the polynomial interpolation seen in the attached PC code.
int distanceCount[] = {  7,  8,  9,  10,  11,  12,  13,  14,  15,  16,  
17,  18,  19,  20,  21,  22,  23,  24,  25,  26,  27,  28,  29,  30,  
31,  32,  33,  34,  35,  36,  37,  38,  39,  40,  41,  42,  43,  44,  
45,  46,  47,  48,  49,  50,  51,  52,  53,  54,  55,  56,  57,  58,  
59,  60,  61,  62,  63,  64,  65,  66,  67,  68,  69, };

int sensorCount[] = {  1015, 932, 860, 799, 747, 702, 664, 630, 601, 
575, 552, 531, 512, 494, 478, 463, 449, 435, 422, 409, 397, 385, 374, 
363, 353, 343, 334, 325, 316, 308, 300, 293, 286, 280, 274, 268, 262, 
257, 253, 248, 244, 240, 236, 232, 229, 225, 222, 219, 215, 212, 209, 
206, 203, 200, 197, 195, 192, 189, 187, 184, 182, 180, 178, };


int averageFiveValues(Counter cx) {
	
	//account for the fact that we may be starting with 1 cx.n value
	int count = 0;
	if (cx.a != 0) count++;
	if (cx.b != 0) count++;
	if (cx.c != 0) count++;
	if (cx.d != 0) count++;
	if (cx.e != 0) count++;

	return (cx.a + cx.b + cx.c + cx.d + cx.e)/count;
}

// { [a] [b] [c] [d] [e] }
Counter shiftValuesToRightAndAddNewBit(Counter cx, int newBit) {
	cx.e = cx.d;
	cx.d = cx.c;
	cx.c = cx.b;
	cx.b = cx.a;
	cx.a = newBit;
	return cx;
}

/* Returns and integer value of the distance
 * found by the infrared sensor.
 */
int findDistance(IRDistanceArrayGroup ag) {
	for(int i = 0; i < COUNTVALUE; i++) {
		//subtract from distance count [i] the offset value per each bot
		int offset = - 2;
		if(sensorCount[i] <= ag.x) return distanceCount[i] - offset;
	}
	
	return 0;
}

//This function sets the data structures defined in types.h to the LUTs
IRDistanceArrayGroup initIRDistanceArrayGroup(IRDistanceArrayGroup self) {

	for(int i = 0; i < COUNTVALUE; i++) {

		self.distanceArray[i] = distanceCount[i];
		self.sensorArray[i] = sensorCount[i];
	}
	return self;
}

